#!/usr/bin/env python
# -*- coding: utf-8 -*-
import random
import subprocess
import rospy
import math
from yahboomcar_msgs.msg import ArmJoint
import time
import cv2
import numpy as np
from cv_bridge import CvBridge
from sensor_msgs.msg import Image, Imu, JointState, LaserScan
from geometry_msgs.msg import Twist
from tf.transformations import euler_from_quaternion
from cv_bridge import CvBridge
from std_msgs.msg import String

# Global variables
is_arm_moving = False
is_car_moving = True
detective_time = False
AsideCnt = 0
# [-1.57 ~ 1.57] [-3.14 ~ 3.14(pi)] [-pi ~ 0]
joints = [0, -0.93, 2.07, 1.3, 0, 0.8]
move_x, move_y, arm_up = 0, 150, 300
mid_block_cx, mid_block_cy = 320, 240  # Image center point
start_flag = 0

# Arm parameters (unit: mm)
l0 = 2100  # Base height
l1 = 1250  # Upper arm length
l2 = 1200  # Forearm length
l3 = 1550  # Wrist length

# Flower detector instance (will be initialized in image_callback)
flower_detector = None


class ImuController:
    def __init__(self):
        self.target_yaw = 0.0  # Target yaw angle (radians)
        self.current_yaw = 0.0  # Current yaw angle (radians)
        self.kp = 0.5  # Proportional control coefficient
        self.threshold = 0.05  # Angle threshold (radians)
        self.angular_vel = 0.0  # Output angular velocity
        # Subscribers and publishers
        self.imu_sub = rospy.Subscriber("/imu", Imu, self.imu_callback)
        self.vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)

    def imu_callback(self, msg):
        """IMU data callback: Extract yaw angle"""
        quaternion = (
            msg.orientation.x,
            msg.orientation.y,
            msg.orientation.z,
            msg.orientation.w,
        )
        # Calculate Euler angles from quaternion (roll, pitch, yaw)
        _, _, yaw = euler_from_quaternion(quaternion)
        self.current_yaw = self.normalize_angle(yaw)  # Normalize angle to [-�, �]

    def normalize_angle(self, angle):
        """Normalize angle to [-�, �] range"""
        while angle > math.pi:
            angle -= 2 * math.pi
        while angle < -math.pi:
            angle += 2 * math.pi
        return angle

    def calculate_control(self):
        """Calculate orientation control output"""
        error = self.target_yaw - self.current_yaw
        error = self.normalize_angle(error)  # Ensure shortest path

        if abs(error) > self.threshold:
            # Proportional control with clamping
            self.angular_vel = np.clip(
                self.kp * error, -0.5, 0.5
            )  # Max angular velocity �0.5 rad/s
        else:
            self.angular_vel = 0.0  # Stop when error below threshold

        return self.angular_vel

    def set_target(self, yaw):
        """Set target yaw angle"""
        self.target_yaw = self.normalize_angle(yaw)


class RobotArm:
    def __init__(self):
        self.arm_pub = rospy.Publisher("/TargetAngle", ArmJoint, queue_size=100)
        self.JOINT_NAMES = [
            "joint1",
            "joint2",
            "joint3",
            "joint4",
            "joint5",
            "grip_joint",
        ]
        self.joints_angle_deg = [90.0, 90.0, 90.0, 90.0, 90.0, 90.0]
        self.joints_angle_rad = [
            0.0,
            0.6,
            -1.57,
            -0.785,
            -1.57,
            -1.38,
        ]  # Initial joint angles
        self.pi = math.pi
        self.current_region_idx = 1
        self.joint_data_radians = {
            (1, 1): [
                -0.9948376736367679,
                0.9250245035569946,
                -1.2566370614359172,
                -1.5707963267948966,
                -0.06981317007977318,
                1.0,
            ],
            (1, 2): [
                -0.9773843811168246,
                0.4188790204786391,
                -1.2566370614359172,
                -1.5707963267948966,
                -0.06981317007977318,
                1.0,
            ],
            (1, 3): [
                -0.9424777960769379,
                0.22689280275926285,
                -1.3962634015954636,
                -1.1519173063162575,
                -0.06981317007977318,
                1.0,
            ],
            (1, 4): [
                0.9773843811168246,
                0.6806784082777885,
                -1.3439035240356338,
                -0.9599310885968813,
                -0.06981317007977318,
                1.0,
            ],
            (1, 5): [
                1.0122909661567112,
                0.3490658503988659,
                -1.3264502315156905,
                -1.0122909661567112,
                -0.06981317007977318,
                1.0,
            ],
            (1, 6): [
                0.9250245035569946,
                0.17453292519943295,
                -1.2740903539558606,
                -1.1868238913561442,
                -0.06981317007977318,
                1.0,
            ],
            (2, 1): [
                -0.8203047484373349,
                0.2792526803190927,
                -1.2217304763960306,
                -0.7504915783575618,
                -0.06981317007977318,
                1.0,
            ],
            (2, 2): [
                -0.8377580409572782,
                0.017453292519943295,
                -0.9075712110370514,
                -1.2217304763960306,
                -0.06981317007977318,
                1.0,
            ],
            (2, 3): [
                -0.8203047484373349,
                -0.3141592653589793,
                -0.7330382858376184,
                -1.413716694115407,
                -0.06981317007977318,
                1.0,
            ],
            (2, 4): [
                0.8901179185171081,
                -0.05235987755982989,
                -0.5759586531581288,
                -1.1868238913561442,
                -0.06981317007977318,
                1.0,
            ],
            (2, 5): [
                0.8203047484373349,
                -0.05235987755982989,
                -0.6108652381980153,
                -1.4835298641951802,
                -0.06981317007977318,
                1.0,
            ],
            (2, 6): [
                0.8203047484373349,
                0.3665191429188092,
                -1.5707963267948966,
                -0.9948376736367679,
                -0.06981317007977318,
                1.0,
            ],
            (3, 1): [
                -0.9773843811168246,
                1.117010721276371,
                -1.5707963267948966,
                -1.3264502315156905,
                -0.06981317007977318,
                1.0,
            ],
            (3, 2): [
                -0.9250245035569946,
                0.767944870877505,
                -1.5707963267948966,
                -1.2915436464758039,
                -0.06981317007977318,
                1.0,
            ],
            (3, 3): [
                -0.7504915783575618,
                0.6283185307179586,
                -1.5707963267948966,
                -1.3962634015954636,
                -0.06981317007977318,
                1.0,
            ],
            (3, 4): [
                0.8726646259971648,
                0.6283185307179586,
                -1.5707963267948966,
                -0.6632251157578453,
                -0.06981317007977318,
                1.0,
            ],
            (3, 5): [
                0.8552113334772214,
                0.5235987755982988,
                -1.2042771838760873,
                -1.3264502315156905,
                -0.06981317007977318,
                1.0,
            ],
            (3, 6): [
                0.767944870877505,
                0.3839724354387525,
                -1.2042771838760873,
                -1.5184364492350666,
                -0.06981317007977318,
                1.0,
            ]
        }

        self.click_data_radians = {
            (3, 6): [
                0.8726646259971648,
                0.08726646259971647,
                -1.2042771838760873,
                -1.0297442586766545,
                -0.06981317007977318,
                -0.16445424531724992,
            ],
            (3, 5): [
                0.8552113334772214,
                0.12217304763960307,
                -1.2042771838760873,
                -0.7155849933176751,
                -0.06981317007977318,
                -0.16445424531724992,
            ],
            (3, 4): [
                0.9075712110370514,
                -0.10471975511965978,
                -0.9424777960769379,
                -0.47123889803846897,
                -0.06981317007977318,
                -0.16445424531724992,
            ],
            (3, 3): [
                -0.767944870877505,
                0.22689280275926285,
                -1.5707963267948966,
                -0.8552113334772214,
                -0.06981317007977318,
                -0.16445424531724992,
            ],
            (3, 2): [
                -0.7504915783575618,
                0.3490658503988659,
                -1.5707963267948966,
                -0.6981317007977318,
                -0.06981317007977318,
                -0.16445424531724992,
            ],
            (3, 1): [
                -0.9250245035569946,
                0.4014257279586958,
                -1.5707963267948966,
                -0.4363323129985824,
                -0.06981317007977318,
                -0.16445424531724992,
            ],
            (2, 6): [
                0.8203047484373349,
                0.19198621771937624,
                -1.5707963267948966,
                -0.6108652381980153,
                -0.06981317007977318,
                -0.16445424531724992,
            ],
            (2, 5): [
                0.8901179185171081,
                -0.29670597283903605,
                -0.6108652381980153,
                -0.9948376736367679,
                -0.06981317007977318,
                -0.16445424531724992,
            ],
            (2, 4): [
                0.8901179185171081,
                -0.29670597283903605,
                -0.5759586531581288,
                -0.7504915783575618,
                -0.06981317007977318,
                -0.16445424531724992,
            ],
            (2, 3): [
                -0.7504915783575618,
                -0.3141592653589793,
                -0.7330382858376184,
                -1.1868238913561442,
                -0.06981317007977318,
                -0.16445424531724992,
            ],
            (2, 2): [
                -0.8028514559173916,
                -0.3665191429188092,
                -0.7330382858376184,
                -0.8726646259971648,
                -0.06981317007977318,
                -0.16445424531724992,
            ],
            (2, 1): [
                -0.8203047484373349,
                -0.4363323129985824,
                -0.8726646259971648,
                -0.19198621771937624,
                -0.06981317007977318,
                -0.16445424531724992,
            ],
            (1, 6): [
                0.9773843811168246,
                0.10471975511965978,
                -1.2740903539558606,
                -0.9075712110370514,
                -0.06981317007977318,
                -0.16445424531724992,
            ],
            (1, 5): [
                1.064650843716541,
                0.05235987755982989,
                -1.2740903539558606,
                -0.5235987755982988,
                -0.06981317007977318,
                -0.16445424531724992,
            ],
            (1, 4): [
                1.0995574287564276,
                -0.20943951023931956,
                -1.3264502315156905,
                0.17453292519943295,
                -0.06981317007977318,
                -0.16445424531724992,
            ],
            (1, 3): [
                -0.9424777960769379,
                0.24434609527920614,
                -1.3962634015954636,
                -0.9250245035569946,
                -0.06981317007977318,
                -0.3424778290206716,
            ],
            (1, 2): [
                -0.9948376736367679,
                0.15707963267948966,
                -1.0995574287564276,
                -0.8203047484373349,
                -0.06981317007977318,
                -0.3424778290206716,
            ],
            (1, 1): [
                -0.9773843811168246,
                0.22689280275926285,
                -1.2042771838760873,
                -0.8203047484373349,
                -0.06981317007977318,
                -0.3424778290206716,
            ]
        }

    def publish_arm(self):
        """Publish arm joint states"""
        # joint_state = JointState()  # Create ROS joint state message
        # joint_state.header.stamp = rospy.Time.now()
        # self.joints_angle_deg = [math.degrees(deg) for deg in self.joints_angle_rad]
        for idx, joint_angle in enumerate(self.joints_angle_deg):
            for _ in range(5):
                arm_joint = ArmJoint()
                arm_joint.id = idx + 1
                arm_joint.run_time = 10
                arm_joint.angle = joint_angle  # unit: radians
                self.arm_pub.publish(arm_joint)
                rospy.sleep(0.15)

    def _list_rad2deg(self, li=[0]):
        try:
            ret = [math.degrees(rad) + 90.0 for rad in li]
        except:
            rospy.logerr("rad2deg error")
            return []
        return ret

    def calculate_deg(self, flower_idx, click=False):
        if click:
            angles = self.click_data_radians[(self.current_region_idx, flower_idx)]
        else:
            angles = self.joint_data_radians[(self.current_region_idx, flower_idx)]
        self.joints_angle_deg = self._list_rad2deg(angles)

    def move_arm(self, flower_idx, click):
        self.calculate_deg(flower_idx, click)
        self.publish_arm()


class RobotController:
    def __init__(self):
        self.cmd_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
        self.imu_ctrl = ImuController()
        # Flower detection subscriber
        self.followDetective = rospy.Subscriber(
            "/usb_cam/image_raw", Image, self.image_callback
        )
        self.arm = RobotArm()
        self.twist = Twist()
        # Lidar subscriber
        self.lidar_sub = rospy.Subscriber("/scan", LaserScan, self.lidar_callback)
        # Initial parameters
        self.danger_distance = 0.31  # Danger distance threshold (meters)
        self.side_angle_range = (
            90  # Total detection angle range (degrees), 45 each side
        )
        self.obstacle_detected = False
        self.left_distance = 0.0  # Left average distance
        self.right_distance = 0.0  # Right average distance
        self.robot_init()

    def image_callback(self, msg):
        """Image processing callback: Detect flowers and distinguish male/female"""
        global start_flag, move_x, move_y, arm_up, target_center, best_flower_type, A_side_flower_sex, flower_detector
        global mid_block_cx, mid_block_cy

        # Initialize flower detector (delayed to ensure ROS node is ready)
        if flower_detector is None:
            flower_detector = FlowerDetector()

        if start_flag == 0:
            rospy.sleep(2)
            start_flag = 1

        # Convert ROS image message to OpenCV format
        try:
            cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
        except Exception as e:
            rospy.logerr("Image conversion error: {}".format(e))
            return

        # Resize image for faster processing (optimization for Raspberry Pi)
        height, width = cv_image.shape[:2]
        if width > 640:
            scale = 640.0 / width
            new_size = (int(width * scale), int(height * scale))
            cv_image = cv2.resize(cv_image, new_size)
            mid_block_cx = int(320 * scale)  # Update image center
            mid_block_cy = int(240 * scale)

        # Detect flowers using FlowerDetector
        flower_type, result_img, center = flower_detector.detect(cv_image)

        # Update global detection results
        best_flower_type = flower_type
        if best_flower_type == "female":
            A_side_flower_sex = "female"
        else:
            A_side_flower_sex = ["male", "female"][random.randint(0, 1)]

        # Draw detection results
        if center:
            # Draw center marker
            cv2.circle(result_img, center, 5, (0, 0, 255), -1)
            # Update target center coordinates
            target_center = center

            # Adjust arm position based on target (scale back to original)
            scale = width / float(result_img.shape[1])  # Calculate scale factor
            real_cx = int(center[0] * scale)
            real_cy = int(center[1] * scale)

            # Horizontal adjustment (x direction)
            if abs(real_cx - mid_block_cx) > 10:
                move_x_adjust = (real_cx - mid_block_cx) * 0.01
                move_x += move_x_adjust

            # Vertical adjustment (y direction, farther objects have larger y)
            if abs(real_cy - mid_block_cy) > 10:
                if real_cy > mid_block_cy and arm_up > 1:
                    arm_up -= (real_cy - mid_block_cy) * 0.02
                else:
                    arm_up += abs(real_cy - mid_block_cy) * 0.02

            # Publish coordinates
            coordinates = (
                "Detected {}: x={}, y={}, move_x={:.2f}, arm_up={:.2f}".format(
                    flower_type, real_cx, real_cy, move_x, arm_up
                )
            )
            color_pub.publish(coordinates)
        else:
            # No flower detected
            target_center = None
            color_pub.publish("未检测到花")

        # Display image (for debugging, can be commented on Raspberry Pi)
        # cv2.imshow("result", result_img)
        # cv2.waitKey(1)

    def robot_init(self):
        """System initialization"""
        # Initialize chassis speed
        self.twist.linear.x = 0
        self.twist.linear.y = 0
        self.twist.angular.z = 0
        self.cmd_pub.publish(self.twist)

        # Initialize arm position
        rate = rospy.Rate(1)  # 1Hz
        rospy.loginfo("Initializing arm position...")
        rate.sleep()
        rospy.sleep(3)  # Wait for arm to move

        # Initialize IMU target angle (use current orientation)
        rospy.loginfo("Initializing IMU reference...")
        rospy.sleep(1)  # Wait for IMU data to stabilize
        self.imu_ctrl.set_target(self.imu_ctrl.current_yaw)
        rospy.loginfo(
            "Target yaw angle: {:.2f}".format(math.degrees(self.imu_ctrl.target_yaw))
        )

    def lidar_callback(self, scan):
        """Lidar data processing: Calculate left/right distances and filter far obstacles"""
        # Calculate angle parameters
        angle_min = scan.angle_min  # Lidar start angle (radians, typically -�)
        angle_increment = scan.angle_increment  # Angle increment per point (radians)
        total_angles = len(scan.ranges)  # Total detection points

        # Convert to symmetric angle range (45� each side)
        half_angle = math.radians(
            self.side_angle_range / 2.0
        )  # Single side angle (radians)
        left_end_angle = (
            -half_angle
        )  # Left boundary angle (0 is front, negative is left)
        right_start_angle = half_angle  # Right boundary angle (positive is right)

        # Calculate left/right boundary indices
        left_end_idx = int((left_end_angle - angle_min) / angle_increment)
        right_start_idx = int((right_start_angle - angle_min) / angle_increment)

        # Extract left/right detection areas, filtering obstacles beyond 1m
        left_ranges = []
        right_ranges = []

        for i, distance in enumerate(scan.ranges):
            # Filter invalid values and obstacles beyond 1m
            if scan.range_min <= distance <= scan.range_max and distance < 1.0:
                angle = angle_min + i * angle_increment
                if angle <= left_end_angle:  # Left area
                    left_ranges.append(distance)
                elif angle >= right_start_angle:  # Right area
                    right_ranges.append(distance)

        # Calculate shortest left/right distances
        self.left_distance = min(left_ranges) if left_ranges else float("inf")
        self.right_distance = min(right_ranges) if right_ranges else float("inf")

        # Output distance info
        rospy.loginfo(
            "左距离: {:.2f}m, 右距离: {:.2f}m".format(
                self.left_distance, self.right_distance
            )
        )

        # Check for close obstacles (within danger distance)
        self.obstacle_detected = any(
            distance < self.danger_distance for distance in left_ranges + right_ranges
        )

    def stabilize_orientation(self):
        """Orientation stabilization control: Maintain robot heading"""
        angular_vel = self.imu_ctrl.calculate_control()
        self.twist.angular.z = angular_vel

    def adjust_steering(self):
        """Adjust steering based on left/right distance difference"""
        # Only adjust when both sides have valid distance data
        if self.right_distance < self.left_distance:
            # Larger difference means higher y speed (max 0.1m/s)
            diff = self.left_distance - self.right_distance
            self.twist.linear.y = np.clip(diff, 0, 0.1)  # Max left speed 0.1m/s
            rospy.loginfo(
                "距离右侧更近，向左调整: y speed={:.2f}m/s".format(self.twist.linear.y)
            )
        elif self.right_distance < self.left_distance:
            diff = self.left_distance - self.right_distance
            self.twist.linear.y = np.clip(diff, 0, -0.1)
            rospy.rospy.loginfo(
                "距离左侧更近，向右调整：y speed={:.2f}m/s".format(self.twist.linear.y)
            )
        else:
            rospy.loginfo("无需调整")
            self.twist.linear.y = 0  # Otherwise y speed is 0

    def run(self):
        """Main control loop"""
        global is_car_moving, is_arm_moving, detective_time, AsideCnt, flower_detector

        rospy.loginfo("Starting robot control system...")
        rate = rospy.Rate(10)  # 10Hz control frequency
        firstTime = time.time()

        while not rospy.is_shutdown():
            if is_car_moving:
                # Chassis movement control
                if (
                    self.obstacle_detected
                    and time.time() - firstTime > 3.5
                    and AsideCnt < 3
                ):
                    rospy.loginfo("Obstacle detected, stopping")
                    self.twist.linear.x = 0
                    self.twist.linear.y = 0
                    self.twist.angular.z = 0
                    self.cmd_pub.publish(self.twist)
                    is_car_moving = False
                    is_arm_moving = True
                else:
                    # Determine movement direction based on state
                    if AsideCnt < 3:
                        # Zone A not complete, move forward
                        self.twist.linear.x = 0.15
                        # self.twist.angular.z = 0.075
                    else:
                        # Zone A complete, stop
                        # self.twist.linear.x = 0.0
                        self.shutdown_hook()
                        # self.twist.angular.z = 0.05

                    # Publish control commands
                    self.cmd_pub.publish(self.twist)
                    rospy.loginfo(
                        "底盘运动: lin_x={:.2f}m/s, lin_y={:.2f}m/s, ang_z={:.2f}rad/s".format(
                            self.twist.linear.x,
                            self.twist.linear.y,
                            self.twist.angular.z,
                        )
                    )
            elif is_arm_moving:
                # Arm operation
                rospy.loginfo("开始操作机械臂...")
                # raw_input("enter anything to continue...")
                self.twist.linear.x = 0
                self.twist.linear.y = 0
                self.twist.angular.z = 0
                self.cmd_pub.publish(self.twist)

                # Record detection results
                detection_results = []

                # Move arm to detection positions
                positions = [
                    (105, 165, 280),  # Lower left
                    (115, 170, 340),  # Middle left
                    (85, 130, 435),  # Upper left
                    (-80, 125, 270),  # Lower right
                    (-95, 140, 340),  # Middle right
                    (-105, 140, 385),  # Upper right
                ]
                global A_side_flower_sex, move_x, move_y, arm_up
                for flower_idx in range(1, 7):
                    rospy.loginfo(
                        "Moving arm to position region: {} flower: {}".format(
                            self.arm.current_region_idx, flower_idx
                        )
                    )
                    self.arm.move_arm(flower_idx, click=False)
                    rospy.sleep(5)  # Allow time for detection

                    if A_side_flower_sex:
                        # Perform different operations based on flower type
                        if A_side_flower_sex == "male":  # Male flower
                            subprocess.call("mpg123 /temp/xionghua.mp3 >> /dev/null", shell=True)
                            rospy.loginfo("雄花，不动")
                        elif A_side_flower_sex == "female":  # Female flower
                            self.arm.move_arm(flower_idx=flower_idx, click=True)
                            subprocess.call("mpg123 /temp/cihua.mp3 >> /dev/null", shell=True)
                            rospy.loginfo(">>>雌花，授粉<<<")
                    else:
                        rospy.loginfo("未识别到花，不动")
                        detection_results.append(A_side_flower_sex)
                    A_side_flower_sex = "male"

                    rospy.sleep(2)  # Wait for operation to complete
                AsideCnt += 1
                self.arm.current_region_idx += 1
                # Return arm to initial position
                # self.arm.move_to(0, 150, 300)
                firstTime = time.time()
                is_arm_moving = False
                is_car_moving = True
                rospy.loginfo("机械臂操作已完成, A 区操作次数: {}".format(AsideCnt))
                rospy.loginfo("识别结果: {}".format(detection_results))

            rate.sleep()

    def shutdown_hook(self):
        """Callback when node shuts down"""
        rospy.loginfo("系统已停止，正在退出...")

        # Stop chassis movement
        self.twist.linear.x = 0
        self.twist.linear.y = 0
        self.twist.angular.z = 0
        self.cmd_pub.publish(self.twist)

        # Return arm to home position
        # self.arm.move_to(0, 150, 300)
        rospy.sleep(2)


# Image processing callback
bridge = CvBridge()
color_pub = rospy.Publisher("color_coordinates", String, queue_size=10)
target_center = None  # Target center coordinates (for arm tracking)
best_flower_type = None  # Record best flower type (female/male)
A_side_flower_sex = "male" # "male" for male, "female" for female, "failed" for failure


class FlowerDetector:
    def __init__(self):
        # Yellow detection parameters (HSV range)
        self.YELLOW_LOWER = np.array([15, 100, 100])  # Yellow lower bound
        self.YELLOW_UPPER = np.array([30, 255, 255])  # Yellow upper bound

        # Female flower parameters (large circle)
        self.FEMALE_MIN_AREA = 800  # Female minimum area
        self.FEMALE_MIN_CIRCULARITY = 0.6  # Minimum circularity

        # Male flower detection parameters
        self.MALE_DETECTION_TIME = 7.0  # If no female detected in 7s, assume male
        self.last_female_detection = 0  # Timestamp of last female detection

    def detect(self, frame):
        current_time = time.time()
        result_img = frame.copy()
        # Step 1: Preprocessing
        blurred = cv2.GaussianBlur(frame, (9, 9), 2)
        hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)

        # Step 2: Extract yellow regions
        mask = cv2.inRange(hsv, self.YELLOW_LOWER, self.YELLOW_UPPER)
        mask = cv2.morphologyEx(
            mask, cv2.MORPH_CLOSE, cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))
        )

        # Step 3: Detect all contours
        _, contours, _ = cv2.findContours(
            mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE
        )

        # Step 4: Prioritize female flower detection (large circles)
        female_detected = False
        for cnt in contours:
            area = cv2.contourArea(cnt)
            if area < self.FEMALE_MIN_AREA:
                continue

            perimeter = cv2.arcLength(cnt, True)
            circularity = 4 * np.pi * area / (perimeter**2 + 1e-5)
            rospy.loginfo("circularity: {}".format(circularity))

            if circularity >= self.FEMALE_MIN_CIRCULARITY:
                rospy.loginfo("into...")
                (x, y), _ = cv2.minEnclosingCircle(cnt)
                center = (int(x), int(y))
                self.last_female_detection = current_time
                female_detected = True
                return "female", result_img, center

        # Step 5: Male flower detection logic
        if not female_detected:
            time_since_last_female = current_time - self.last_female_detection
            if time_since_last_female >= self.MALE_DETECTION_TIME:
                # Use largest yellow region as male flower
                if contours:
                    largest_cnt = max(contours, key=cv2.contourArea)
                    # (x, y), _ = cv2.minEnclosingCircle(largest_cnt)
                    x, y, w, h = cv2.boundingRect(largest_cnt)
                    # 绘制矩形框（绿色，线宽2）
                    cv2.rectangle(result_img, (x, y), (x + w, y + h), (0, 255, 0), 2)
                    center = (int(x + w / 2), int(y + h / 2))
                    # 标记中心点（红色圆点）
                    cv2.circle(result_img, center, 3, (0, 0, 255), -1)
                    # Draw male marker (rectangle)
                    return "male", result_img, center
        return "failed", result_img, None


if __name__ == "__main__":
    rospy.init_node("yahboom_robot", anonymous=True)
    rate = rospy.Rate(1)  # 1Hz
    controller = RobotController()
    rospy.on_shutdown(controller.shutdown_hook)
    try:
        controller.run()
    except rospy.ROSInterruptException:
        pass
    finally:
        rospy.loginfo("已退出")
    rospy.spin()
